#include "ros_bridge.h"

#include <cinttypes>

#include <tbox/base/log.h>
#include <tbox/base/json.hpp>
#include <tbox/base/recorder.h>

extern rclcpp::Executor::WeakPtr g_ros_exec_wptr;

namespace your {

RosBridge::RosBridge(tbox::main::Context &ctx, Parent &parent) :
  tbox::main::Module("ros", ctx),
  parent_(parent),
  worker_(ctx.loop()),
  node_(std::make_shared<rclcpp::Node>("tbox"))
{
  using namespace std::placeholders;

  ready_timer_ = node_->create_wall_timer(std::chrono::milliseconds(1), std::bind(&RosBridge::onTimerTick, this));
  pub_string_ = node_->create_publisher<std_msgs::msg::String>("topic_text", 10);
  sub_bool_ = node_->create_subscription<std_msgs::msg::Bool>("topic_bool", 1, std::bind(&RosBridge::onTopicSubBool, this, _1));
  add_two_ints_client_ = node_->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
  add_two_ints_service_ = node_->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", std::bind(&RosBridge::onServiceAddTwoInts, this, _1, _2, _3));
}

RosBridge::~RosBridge() { }

void RosBridge::onFillDefaultConfig(tbox::Json &js_this) { }

bool RosBridge::onInit(const tbox::Json &js_this) { return true; }

bool RosBridge::onStart() {
  auto exec = g_ros_exec_wptr.lock();
  if (exec) {
    exec->add_node(node_);
    return true;

  } else {
    LogErr("exec == nullptr");
    return false;
  }
}

void RosBridge::onStop() { }

void RosBridge::toJson(tbox::Json &js) const { }

void RosBridge::pubText(const std::string &text) {
  RECORD_SCOPE();
  std_msgs::msg::String msg;
  msg.data = text;

  //! 委托给Worker线程去发布，防止阻塞Loop线程
  worker_.execute(
    [this, msg] () {
      RECORD_SCOPE();
      pub_string_->publish(msg);
    }
  );
}

void RosBridge::addTwoInt(int a, int b, AddTwoIntCallback &&cb) {
  RECORD_SCOPE();
  auto req = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
  req->a = a;
  req->b = b;

  //! 交给Worker线程去请求，防止Loop线程被阻塞
  worker_.execute([this, req, cb = std::move(cb)] {
    RECORD_SCOPE();
    add_two_ints_client_->async_send_request(req,
      [this, cb = std::move(cb)] (rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture future) {
        RECORD_SCOPE();
        auto result = future.get();
        //! 移交给Loop线程去处理，防止ROS线程操作外部资源
        ctx().loop()->runInLoop(
          [result, cb = std::move(cb)] {
            if (result) {
              cb(true, result->sum);
            } else {
              cb(false, 0);
            }
          }
        );
      }
    );
  });
}

void RosBridge::onTimerTick() {
  RECORD_SCOPE();
  ready_timer_->cancel();
  //! 转交给Loop线程处理，防止ROS线程操作外部资源
  ctx().loop()->runInLoop(
    [this] () {
      RECORD_SCOPE();
      parent_.on_Ros_ready();
    }
  );
}

void RosBridge::onTopicSubBool(const std_msgs::msg::Bool::SharedPtr msg) {
  RECORD_SCOPE();
  //! 转交给Loop线程处理，防止ROS线程操作外部资源
  ctx().loop()->runInLoop(
    [this, msg] () {
      RECORD_SCOPE();
      parent_.on_Ros_gotBool(msg->data);
    }
  );
}

//! 这里演示的是需要异步处理的情况（同步情况按ROS示例即可）
//! 模拟ROS线程在收到服务请求之后，需要转交给Loop线程进行处理的情况
//! 由于Loop处理事件都是非阻塞的，所以要使用 std::promise 等待上层在完成处理后设置结果
void RosBridge::onServiceAddTwoInts(const std::shared_ptr<rmw_request_id_t> request_header,
                                    const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> req,
                                    const std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> rsp) {
  LogInfo("request: %" PRId64 " + %" PRId64, req->a, req->b);
  std::promise<int> p;
  auto f = p.get_future();

  //! 转交由Loop线程去进行异步处理
  ctx().loop()->runInLoop([this, req, rsp, &p] {
    parent_.on_Ros_addTwoInts(req->a, req->b, p);
  });

  f.wait_for(std::chrono::seconds(5));
  rsp->sum = f.get();
  LogInfo("respond: %" PRId64, rsp->sum);
}

//! 讲解说明

//! RECORD_SCOPE() 不是必须的，它只是用于记录函数调用的性能数据，便于调试和优化。
//! 你可以根据需要选择是否使用它。如果不需要性能记录，可以将其删除或注释掉。

//! 上面的 pubText() 与 addTwoInt() 中都使用了 worker_.execute(...)，是为了防止Loop线程被阻塞；
//! 也可以使用 ctx().thread_pool()->execute(...) 使用线程池来执行，但线程池的执行是不保障顺序性的。
//! 为此，我们推荐使用 WorkThread。但 WorkThread 内部只有一个线程在工作，如果同时委派多个任务，则会排队。

//! 上面的 addTwoInt(), onTopicSubBool(), onServiceAddTwoInts() 中ROS的回调，都是在ROS线程执行的。
//! 如果要上层处理，就必须要使用 ctx().loop()->runInLoop(...) 将事务转交给Loop线程；
//! 如果不使用 runInLoop() 进行转交，让ROS线程直接调用上层函数，则会导致Loop线程与ROS线程同时操控资源，
//! 会引起多线程竞态问题。所以，一定要进行所谓的 “线程隔离”。

}
